model_version = 'v0.0.1';
model_name = 'VTOL Controller';

%% load configuration
% load('control_default_config_arover.mat');
load('control_default_config.mat');

%% Constant Variable
CONTROL_CONST.dt = 0.002;   % model execution period

%% Exported Value
CONTROL_EXPORT_VALUE.period = uint32(CONTROL_CONST.dt*1e3);
CONTROL_EXPORT_VALUE.model_info = int8([model_name, ' ', model_version, 0]); % 0 for end of string
% Export to firmware
CONTROL_EXPORT = Simulink.Parameter(CONTROL_EXPORT_VALUE);
CONTROL_EXPORT.CoderInfo.StorageClass = 'ExportedGlobal';

% Airframe id
AIRFRAME = 1;

%% Paramaters
CONTROL_PARAM_VALUE.MC_VEL_XY_P = single(1.4);
CONTROL_PARAM_VALUE.MC_VEL_XY_I = single(0.2);
CONTROL_PARAM_VALUE.MC_VEL_XY_D = single(0.2);
CONTROL_PARAM_VALUE.MC_VEL_Z_P = single(0.6);
CONTROL_PARAM_VALUE.MC_VEL_Z_I = single(0.1);
CONTROL_PARAM_VALUE.MC_VEL_Z_D = single(0);
CONTROL_PARAM_VALUE.MC_VEL_XY_I_MIN = single(-1);
CONTROL_PARAM_VALUE.MC_VEL_XY_I_MAX = single(1);
CONTROL_PARAM_VALUE.MC_VEL_XY_D_MIN = single(-1);
CONTROL_PARAM_VALUE.MC_VEL_XY_D_MAX = single(1);
CONTROL_PARAM_VALUE.MC_VEL_Z_I_MIN = single(-0.2);
CONTROL_PARAM_VALUE.MC_VEL_Z_I_MAX = single(0.2);
CONTROL_PARAM_VALUE.MC_VEL_Z_D_MIN = single(-0.1);
CONTROL_PARAM_VALUE.MC_VEL_Z_D_MAX = single(0.1);
CONTROL_PARAM_VALUE.MC_ROLL_P = single(5);
CONTROL_PARAM_VALUE.MC_PITCH_P = single(5);
CONTROL_PARAM_VALUE.MC_ROLL_PITCH_CMD_LIM = single(pi/9);

CONTROL_PARAM_VALUE.MC_ROLL_RATE_P = single(0.3);
CONTROL_PARAM_VALUE.MC_PITCH_RATE_P = single(0.3);
CONTROL_PARAM_VALUE.MC_YAW_RATE_P = single(0.6);
CONTROL_PARAM_VALUE.MC_ROLL_RATE_I = single(0.3);
CONTROL_PARAM_VALUE.MC_PITCH_RATE_I = single(0.3);
CONTROL_PARAM_VALUE.MC_YAW_RATE_I = single(0.4);
CONTROL_PARAM_VALUE.MC_ROLL_RATE_D = single(0.009);
CONTROL_PARAM_VALUE.MC_PITCH_RATE_D = single(0.009);
CONTROL_PARAM_VALUE.MC_YAW_RATE_D = single(0.002);
CONTROL_PARAM_VALUE.MC_RATE_I_MIN = single(-0.25);
CONTROL_PARAM_VALUE.MC_RATE_I_MAX = single(0.25);
CONTROL_PARAM_VALUE.MC_RATE_D_MIN = single(-0.25);
CONTROL_PARAM_VALUE.MC_RATE_D_MAX = single(0.25);
CONTROL_PARAM_VALUE.MC_P_Q_CMD_LIM = single(pi/2);
CONTROL_PARAM_VALUE.MC_R_CMD_LIM = single(pi);
CONTROL_PARAM_VALUE.MC_HOVER_THRO = single(0.5);
CONTROL_PARAM_VALUE.MC_OUT_MAX = uint16(2000);
CONTROL_PARAM_VALUE.MC_OUT_MIN = uint16(1000);
CONTROL_PARAM_VALUE.MC_FACTOR_OFS = single(0.5);

% CONTROL_PARAM_VALUE.FW_ROLL_P = single(10);
% CONTROL_PARAM_VALUE.FW_PITCH_P = single(20);
CONTROL_PARAM_VALUE.FW_ROLL_P = single(7);
CONTROL_PARAM_VALUE.FW_PITCH_P = single(7);
CONTROL_PARAM_VALUE.FW_ROLL_PITCH_CMD_LIM = single(pi/3);

% CONTROL_PARAM_VALUE.FW_ROLL_RATE_P = single(0.1);
% CONTROL_PARAM_VALUE.FW_PITCH_RATE_P = single(1);
CONTROL_PARAM_VALUE.FW_ROLL_RATE_P = single(0.1);
CONTROL_PARAM_VALUE.FW_PITCH_RATE_P = single(0.2);
CONTROL_PARAM_VALUE.FW_YAW_RATE_P = single(0.2);
CONTROL_PARAM_VALUE.FW_ROLL_RATE_I = single(0.1);
CONTROL_PARAM_VALUE.FW_PITCH_RATE_I = single(0.1);
CONTROL_PARAM_VALUE.FW_YAW_RATE_I = single(0.2);
CONTROL_PARAM_VALUE.FW_RATE_I_MIN = single(-0.3);
CONTROL_PARAM_VALUE.FW_RATE_I_MAX = single(0.3);
CONTROL_PARAM_VALUE.FW_P_Q_CMD_LIM = single(pi/2);
CONTROL_PARAM_VALUE.FW_R_CMD_LIM = single(pi);
CONTROL_PARAM_VALUE.FW_YAW_RATE_LIM = single(pi/4);

CONTROL_PARAM_VALUE.FW_AIRSPEED_TRIM = single(17);
CONTROL_PARAM_VALUE.FW_FF = single(0.2);
% CONTROL_PARAM_VALUE.FW_FF_LIMIT = single(0.3);
% CONTROL_PARAM_VALUE.FW_PI_LIMIT = single(1);
CONTROL_PARAM_VALUE.FW_ROLL_EFFC = single(1);
CONTROL_PARAM_VALUE.FW_PITCH_EFFC = single(1);
CONTROL_PARAM_VALUE.FW_YAW_EFFC = single(1);
CONTROL_PARAM_VALUE.FW_PITCH_OFFSET = single(0/180*pi);
CONTROL_PARAM_VALUE.FW_CRUISE_THRO = single(0.5);

CONTROL_PARAM_VALUE.FW_TECS_PITCH_F = single(0.1);
CONTROL_PARAM_VALUE.FW_TECS_THOR_FF = single(0.1);
% CONTROL_PARAM_VALUE.FW_TECS_PITCH_P = single(0.1);
CONTROL_PARAM_VALUE.FW_TECS_PITCH_P = single(0.15);
% CONTROL_PARAM_VALUE.FW_TECS_THOR_P = single(0.1);
CONTROL_PARAM_VALUE.FW_TECS_THOR_P = single(0.5);
% CONTROL_PARAM_VALUE.FW_TECS_PITCH_I = single(0.1);
CONTROL_PARAM_VALUE.FW_TECS_PITCH_I = single(0.2);
% CONTROL_PARAM_VALUE.FW_TECS_PITCH_I_LIM = single(0.2);
CONTROL_PARAM_VALUE.FW_TECS_PITCH_I_LIM = single(0.2);
CONTROL_PARAM_VALUE.FW_TECS_THOR_I = single(0.2);
CONTROL_PARAM_VALUE.FW_TECS_PITCH_D = single(0);
CONTROL_PARAM_VALUE.FW_TECS_THOR_D = single(0);
CONTROL_PARAM_VALUE.FW_TECS_VEL_P = single(1);
CONTROL_PARAM_VALUE.FW_TECS_RATIO = single(1);
CONTROL_PARAM_VALUE.FW_TECS_SWITCH = single(2);
CONTROL_PARAM_VALUE.FW_TECS_ANSW = single(2);
CONTROL_PARAM_VALUE.FW_TECS_W2T = single(1);
CONTROL_PARAM_VALUE.FW_TECS_U2T = single(1);
CONTROL_PARAM_VALUE.FW_TECS_W2P = single(2);
% CONTROL_PARAM_VALUE.FW_TECS_U2P = single(0.1);
CONTROL_PARAM_VALUE.FW_TECS_U2P = single(0.2); %%%%
% CONTROL_PARAM_VALUE.FW_TECS_R2P = single(0.1);
% CONTROL_PARAM_VALUE.FW_TECS_R2T = single(0.1);
CONTROL_PARAM_VALUE.FW_TECS_R2P = single(0.0);
CONTROL_PARAM_VALUE.FW_TECS_R2T = single(0.0);
CONTROL_PARAM_VALUE.FW_OUT_MAX = uint16(2000);
CONTROL_PARAM_VALUE.FW_OUT_MIN = uint16(1000);

CONTROL_PARAM_VALUE.FW_AILERON1_DIR = single(1);
CONTROL_PARAM_VALUE.FW_AILERON2_DIR = single(1);
CONTROL_PARAM_VALUE.FW_ELEVATOR_DIR = single(1);
CONTROL_PARAM_VALUE.FW_RUDDER_DIR = single(1);
CONTROL_PARAM_VALUE.FW_TAIL1_DIR = single(1);
CONTROL_PARAM_VALUE.FW_TAIL2_DIR = single(1);
CONTROL_PARAM_VALUE.SERVO1_BIAS = single(0);
CONTROL_PARAM_VALUE.SERVO2_BIAS = single(0);
CONTROL_PARAM_VALUE.SERVO3_BIAS = single(0);
CONTROL_PARAM_VALUE.SERVO4_BIAS = single(0);

% Export to firmware
CONTROL_PARAM = Simulink.Parameter(CONTROL_PARAM_VALUE);
CONTROL_PARAM.CoderInfo.StorageClass = 'ExportedGlobal';